import numpy as np
import plotly.graph_objects as go
import plotly.express as px
import sys
import rtde.csv_reader as csv_reader
import time
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

with open('robot_data.csv') as csvfile:
    r = csv_reader.CSVReader(csvfile)
    cartesian_values=[r.actual_TCP_pose_0,r.actual_TCP_pose_1,r.actual_TCP_pose_2,r.actual_TCP_pose_3,r.actual_TCP_pose_4,r.actual_TCP_pose_5]      
    print(cartesian_values)

[array([-0.60526325, -0.60526001, -0.60525651, ..., -0.44856582,
       -0.44857453, -0.4485674 ]), array([-0.13302554, -0.13302607, -0.13302892, ...,  0.15225299,
        0.15224319,  0.15225249]), array([0.45287712, 0.45285522, 0.45287432, ..., 0.0988814 , 0.09887649,
       0.09888018]), array([-1.20990626, -1.20993065, -1.20992731, ..., -1.25630534,
       -1.25635347, -1.25628789]), array([-1.20977499, -1.20981319, -1.20978161, ..., -1.12484395,
       -1.12483771, -1.1248527 ]), array([1.20789716, 1.20790608, 1.20791768, ..., 1.12224863, 1.12230979,
       1.12225389])]

fig = go.Figure()
Plot1 = go.Scatter3d(x = cartesian_values[0],
                      y = cartesian_values[1],
                      z = cartesian_values[2],                  
                      marker=dict(
                      size=1,
                      color='blue',               
                      colorscale='Viridis',  
                      line=dict(width=1,color='DarkSlateGrey'),
                      opacity=1))
fig.add_trace(Plot1)
fig.show()

with open('robot_data_shy.csv') as csvfile:
    r = csv_reader.CSVReader(csvfile)
    cartesian_values_shy=[r.actual_TCP_pose_0,r.actual_TCP_pose_1,r.actual_TCP_pose_2,r.actual_TCP_pose_3,r.actual_TCP_pose_4,r.actual_TCP_pose_5]      
    print(cartesian_values_shy)

[array([-0.7310207 , -0.73147964, -0.73194498, ..., -0.43400801,
       -0.43398289, -0.43396726]), array([ 0.20498077,  0.20336081,  0.20179995, ..., -0.76283687,
       -0.76284264, -0.76285041]), array([0.08413073, 0.08411702, 0.08417935, ..., 0.10171345, 0.10170047,
       0.10171495]), array([-1.00762399, -1.00632499, -1.00498568, ..., -0.87979928,
       -0.87978456, -0.87972022]), array([-1.41351304, -1.41494492, -1.41625161, ..., -1.62678656,
       -1.62683818, -1.62686472]), array([1.20672263, 1.20830367, 1.20980085, ..., 2.02883616, 2.02883904,
       2.02886329])]

fig = go.Figure()
Plot2 = go.Scatter3d(x = cartesian_values_shy[0],
                      y = cartesian_values_shy[1],
                      z = cartesian_values_shy[2],                  
                      marker=dict(
                      size=1,
                      color='blue',               
                      colorscale='Viridis',  
                      line=dict(width=1,color='DarkSlateGrey'),
                      opacity=1))
fig.add_trace(Plot2)
fig.show()

with open('robot_data_shy2.csv') as csvfile:
    r = csv_reader.CSVReader(csvfile)
    cartesian_values_shy2=[r.actual_TCP_pose_0,r.actual_TCP_pose_1,r.actual_TCP_pose_2,r.actual_TCP_pose_3,r.actual_TCP_pose_4,r.actual_TCP_pose_5]      
    print(cartesian_values_shy2)

[array([-0.60534015, -0.60534915, -0.6053437 , ..., -0.53781871,
       -0.53781409, -0.53782407]), array([-0.13308492, -0.133082  , -0.13308199, ..., -0.5972056 ,
       -0.5972183 , -0.59720526]), array([0.45265782, 0.45264594, 0.45265808, ..., 0.04740181, 0.04743276,
       0.04743658]), array([-1.21034029, -1.21038303, -1.21035688, ..., -0.83529211,
       -0.83527576, -0.83528605]), array([-1.20990327, -1.20990282, -1.20987664, ..., -1.80777571,
       -1.80774313, -1.80773409]), array([1.20819791, 1.20815495, 1.20817173, ..., 1.63440435, 1.63447073,
       1.63446441])]

fig = go.Figure()
Plot3 = go.Scatter3d(x = cartesian_values_shy2[0],
                      y = cartesian_values_shy2[1],
                      z = cartesian_values_shy2[2],                  
                      marker=dict(
                      size=1,
                      color='blue',               
                      colorscale='Viridis',  
                      line=dict(width=1,color='DarkSlateGrey'),
                      opacity=1))
fig.add_trace(Plot3)
fig.show()

with open('robot_data_ash.csv') as csvfile:
    r = csv_reader.CSVReader(csvfile)
    cartesian_values_ash=[r.actual_TCP_pose_0,r.actual_TCP_pose_1,r.actual_TCP_pose_2,r.actual_TCP_pose_3,r.actual_TCP_pose_4,r.actual_TCP_pose_5]      
    print(cartesian_values_ash)

[array([-0.60444674, -0.60444021, -0.60443703, ..., -0.58963776,
       -0.58966091, -0.58965177]), array([-0.1358288 , -0.13584074, -0.13583549, ..., -0.18741559,
       -0.18739888, -0.18740256]), array([0.44791044, 0.4479009 , 0.44789397, ..., 0.53885324, 0.538831  ,
       0.53882678]), array([-1.21491712, -1.21492884, -1.21496894, ..., -0.88058802,
       -0.88064749, -0.88063521]), array([-1.21531753, -1.21534409, -1.21531116, ..., -1.59155989,
       -1.59156158, -1.59159986]), array([1.20771173, 1.2077091 , 1.20767633, ..., 0.90501469, 0.90497203,
       0.90497326])]

fig = go.Figure()
Plot4 = go.Scatter3d(x = cartesian_values_ash[0],
                      y = cartesian_values_ash[1],
                      z = cartesian_values_ash[2],                  
                      marker=dict(
                      size=1,
                      color='blue',               
                      colorscale='Viridis',  
                      line=dict(width=1,color='DarkSlateGrey'),
                      opacity=1))
fig.add_trace(Plot4)
fig.show()